
const int Rdim = 768;
const int Tdim = 1024;
const int TrkNo = 4; // tracker No.
const int XYdim = 2; // X/Y
const double factor = 1;
const double rm = 200 / factor; // curvature of the spherical mirror, unit: cm
const double HitCut = 2.0;      // unit: cm

//parameters to solve the quartic function
const double PI = 3.1415926535897932384626;
const double M_2PI = 2 * PI;
const double eps = 1e-12;

double Xdir, Ydir, Zdir;
double Xinj, Yinj, Zinj;

double DichotomyRecursion(double Omega, double re, double r, double rm)
{
    // dichtomy recursion to solve the angular function
    double high = 200 / 1000;
    double low = 0 / 1000;
    double angle = 100;
    angle /= 1000; // initial value is 100 mrad
    double Omegae = angle;
    double func;
    func = Omegae + asin(re * sin(Omegae) / r) - 2 * asin(re * sin(Omegae) / rm) - Omega;
    //cout << "Initialization : func = " << func << "  Omegae = " << Omegae << "  Omega = " << Omega << "  re = " << re << "  r = " << r << "  rm = " << rm << endl;
    while (abs(func) > 1e-10)
    {
        if (func > 0)
            high = Omegae;
        else if (func < 0)
            low = Omegae;
        Omegae = (low + high) / 2;
        func = Omegae + asin(re * sin(Omegae) / r) - 2 * asin(re * sin(Omegae) / rm) - Omega;

        if (low == high)
            break;
        //cout << "High = " << high << "    low = " << low << endl;
    }

    return Omegae;
}

vector<double> AngleCalculation(vector<pair<double, double>> HitsPosition)
{
    vector<double> Angle;
    // in the coordinates transform ,the X/Y are the same in the Cosmic Ray test coordiante and center-of-curvature coordinate; only the Z axis is different
    // the hit position is the ionization point in the photon detector, obtained by extrapolating the cosmic rat tracking
    double RHitX = (Xinj + 15 * Xdir / Zdir) / factor, RHitY = (Yinj + 15 * Ydir / Zdir) / factor; // unit: cm, the Cosmic Ray test coordinates and the center-of-curvature coordinates
    double RHitZ = 100 / factor;                                                                   // unit: cm, the center-of-curvature coordinates
    double EmitX = 50 * Xdir / Zdir / factor, EmitY = 50 * Ydir / Zdir / factor;                   // unit: cm, the Cosmic Ray test coordinates and the center-of-curvature coordinates
    double EmitZ = 150 / factor;                                                                   // unit: cm, the center-of-curvature coordinates
    //cout << "First EmitX/Y/Z = " << EmitX << "  " << EmitY << "  " << EmitZ << endl;
    double r = 0;        // the distance between center-of-curvature and detection point
    double re = 0;       // the distance between center-of-curvature and photon emission point
    double Cosomega = 0; // angle omega is that of the photon emission tracing and photon detection tracing to the center-of-curvature
    double Sinomega = 0;
    double Omegae = 0, Omegaa = 0, Thetam = 0;

    for (int i = 0; i < HitsPosition.size(); i++)
    {
        double X = HitsPosition[i].first, Y = HitsPosition[i].second;
        //cout << "X = " << X << "  Y = " << Y << endl;

        // emit tracing
        double DeteVecX = X / factor, DeteVecY = Y / factor, DeteVecZ = 100 / factor; // unit: cm, detection point
        r = sqrt(pow(DeteVecX, 2) + pow(DeteVecY, 2) + pow(DeteVecZ, 2));
        double EmitVecX = EmitX, EmitVecY = EmitY, EmitVecZ = (r + rm) / 2; // unit: cm, emit point
        //re = sqrt(pow(EmitX,2) + pow(EmitY,2) + pow(EmitZ,2));
        re = sqrt(pow(EmitVecX, 2) + pow(EmitVecY, 2) + pow(EmitVecZ, 2));
        Cosomega = (EmitVecX * DeteVecX + EmitVecY * DeteVecY + EmitVecZ * DeteVecZ) / re / r;
        if ((EmitVecX * DeteVecX + EmitVecY * DeteVecY) < 0)
            Sinomega = -sqrt(1 - pow(Cosomega, 2));
        else
            Sinomega = sqrt(1 - pow(Cosomega, 2));

        double Thetap = 0, Phip = 0;
        Thetap = acos(Zdir / sqrt(pow(Xdir, 2) + pow(Ydir, 2) + pow(Zdir, 2)));

        if (Xdir != 0)
        {
            if (Xdir < 0)
            {
                if (Ydir > 0)
                    Phip = acos(Xdir / sqrt(pow(Xdir, 2) + pow(Ydir, 2)));
                else if (Ydir < 0)
                    Phip = 2 * PI - acos(Xdir / sqrt(pow(Xdir, 2) + pow(Ydir, 2)));
                else if (Ydir == 0)
                    Phip = PI;
            }
            else if (Xdir > 0)
            {
                if (Ydir > 0)
                    Phip = acos(Xdir / sqrt(pow(Xdir, 2) + pow(Ydir, 2)));
                else if (Ydir < 0)
                    Phip = 2 * PI - acos(Xdir / sqrt(pow(Xdir, 2) + pow(Ydir, 2)));
                else if (Ydir == 0)
                    Phip = 0;
            }
        }
        else
        {
            if (Ydir > 0)
                Phip = PI / 2;
            else
                Phip = 3 * PI / 2;
        }

        //cout << "Thetap = " << Thetap << "  Phip = " << Phip << endl;

        double apz = cos(Thetap), apx = sin(Thetap) * cos(Phip), apy = sin(Thetap) * sin(Phip);
        double bpz = -apx, bpx = pow(sin(Phip), 2) + cos(Thetap) * pow(cos(Phip), 2), bpy = (cos(Thetap) - 1) * cos(Phip) * sin(Phip);
        double cpz = -apy, cpx = bpy, cpy = pow(cos(Phip), 2) + cos(Thetap) * pow(sin(Phip), 2);

        double DR = DichotomyRecursion(acos(Cosomega), re, r, rm);

        Omegae = DR;
        double u = DeteVecZ * apz + DeteVecX * apx + DeteVecY * apy;
        double ue = EmitVecZ * apz + EmitVecX * apx + EmitVecY * apy;
        double Costheta = sin(Omegae) / Sinomega * u / r - sin(Omegae - acos(Cosomega)) / Sinomega * ue / re;
        //cout << "      Cherenkov Angle = " << acos(Costheta)*1000 << endl;
        Angle.push_back(acos(Costheta) * 1000);
    }

    return Angle;
}

void rec()
{
    double Final_y = 1., Final_z = 1.; //!!!in cm!!!
    vector<double> result;             //reconstruction result

    Xdir = -0.0; //
    Ydir = -0.0; //
    Zdir = 1;
    Xinj = 0. / factor; // if factor = 1, unit: cm  //
    Yinj = 0. / factor; //
    Zinj = 0 / factor;

    vector<pair<double, double>> HitsPosition;
    HitsPosition.push_back(make_pair(Final_y / 10, Final_z / 10));
    result = AngleCalculation(HitsPosition);
    cout << result[0] << endl;
}